#define BLINKER_PRINT Serial
#define BLINKER_BLE

#include <Blinker.h>
#include <Robojax_L298N_DC_motor.h>

BlinkerButton ButtonGo("btn-go");
BlinkerButton ButtonBack("btn-back");
BlinkerButton ButtonStop("btn-stop");
BlinkerButton ButtonLeft("btn-left");
BlinkerButton ButtonRight("btn-right");
BlinkerSlider SliderRunSpeed("ran-run-speed");
BlinkerSlider SliderTurnSpeed("ran-turn-speed");
BlinkerSlider SliderWormSpeed("ran-worm-speed");

// motor 1 settings
#define CHA 0
#define ENA 15 // this pin must be PWM enabled pin if Arduino board is used
#define IN1 25
#define IN2 26

// motor 2 settings
#define IN3 12
#define IN4 13
#define ENB 14// this pin must be PWM enabled pin if Arduino board is used
#define CHB 1

const int CCW = 2; // do not change
const int CW  = 1; // do not change
bool motor1Run=false;
bool motor2Run=false;
int runSpeed=70;
int turnSpeed=70;
int wormSpeed=100;

#define motor1 1 // do not change
#define motor2 2 // do not change

Robojax_L298N_DC_motor robot(IN1, IN2, ENA, CHA,  IN3, IN4, ENB, CHB);


// 按下按键即会执行该函数
void buttonGoCallback(const String & state) {
    Serial.println("go");
    if(state=="tap"){
      motor1Run=true;
      robot.rotate(motor1, runSpeed, CW);//CW正转 CCW反转 80=转速
      delay(wormSpeed);
      robot.brake(1);
      motor1Run=false;
    }
    if(state=="press" && motor1Run==false){
      motor1Run = true;
      robot.rotate(motor1, runSpeed, CW);//CW正转 CCW反转 80=转速
    }
    if(state=="pressup" && motor1Run==true){
      robot.brake(3);
      motor1Run = false;
      motor2Run = false;
    }
}

void buttonBackCallback(const String & state) {
    Serial.println("back");
    if(state=="tap"){
      motor1Run=true;
      robot.rotate(motor1, runSpeed, CCW);//CW正转 CCW反转 80=转速
      delay(wormSpeed);
      robot.brake(1);
      motor1Run=false;
    }
    if(state=="press" && motor1Run==false){
      motor1Run = true;
      robot.rotate(motor1, runSpeed, CCW);//CW正转 CCW反转 80=转速
    }
    if(state=="pressup" && motor1Run==true){
      robot.brake(3);
      motor1Run = false;
      motor2Run = false;
    }
}

void buttonLeftCallback(const String & state) {
    Serial.println("left");
    if(state=="tap"){
      motor2Run=true;
      robot.rotate(motor2, turnSpeed, CW);//CW正转 CCW反转 80=转速
      delay(wormSpeed);
      robot.rotate(motor2, turnSpeed, CCW);
      delay(wormSpeed);
      robot.brake(2);
      motor2Run=false;
    }
    if(state=="press"  && motor2Run==false){
      motor2Run = true;
      robot.rotate(motor2, turnSpeed, CW);//CW正转 CCW反转 80=转速
    }
    if(state=="pressup" && motor2Run==true){
      robot.brake(2);//CW正转 CCW反转 80=转速
      motor2Run = false;
    }
}

void buttonRightCallback(const String & state) {
    Serial.println("right");
    if(state=="tap"){
      motor2Run=true;
      robot.rotate(motor2, turnSpeed, CCW);//CW正转 CCW反转 80=转速
      delay(wormSpeed);
      robot.rotate(motor2, turnSpeed, CW);
      delay(wormSpeed);
      robot.brake(2);
      motor2Run=false;
    }
    if(state=="press"  && motor2Run==false){
      motor2Run = true;
      robot.rotate(motor2, turnSpeed, CCW);//CW正转 CCW反转 80=转速
    }
    if(state=="pressup" && motor2Run==true){
      robot.brake(2);//CW正转 CCW反转 80=转速
      motor2Run = false;
    }
}

void buttonStopCallback(const String & state) {
    Serial.println("stop");
    robot.stop(3);
    delay(1000);
    robot.brake(3);
    motor1Run = false;
    motor2Run = false;
}

void SliderRunSpeedCallback(int32_t value)
{
    Serial.println("set speed");
    robot.brake(3);
    motor1Run = false;
    motor2Run = false;
    runSpeed = value;
}

void SliderTurnSpeedCallback(int32_t value)
{
    Serial.println("set speed");
    robot.brake(3);
    motor1Run = false;
    motor2Run = false;
    turnSpeed = value;
}

void SliderWormSpeedCallback(int32_t value)
{
    Serial.println("set worm speed");
    motor1Run = false;
    motor2Run = false;
    robot.brake(3);
    wormSpeed = value;
}

void heartbeat()
{
  SliderRunSpeed.print(runSpeed);
  SliderTurnSpeed.print(turnSpeed);
  SliderWormSpeed.print(wormSpeed);
}

void setup() {
  Serial.begin(115200);
  robot.begin();
  //打开调试模式
  // BLINKER_DEBUG.stream(Serial);
  //输出所有调试信息
  // BLINKER_DEBUG.debugAll();
  //L298N DC Motor by Robojax.com
  Blinker.begin();
  ButtonGo.attach(buttonGoCallback);
  ButtonBack.attach(buttonBackCallback);
  ButtonStop.attach(buttonStopCallback);
  ButtonLeft.attach(buttonLeftCallback);
  ButtonRight.attach(buttonRightCallback);
  SliderRunSpeed.attach(SliderRunSpeedCallback);
  SliderTurnSpeed.attach(SliderTurnSpeedCallback);
  SliderWormSpeed.attach(SliderWormSpeedCallback);
  Blinker.attachHeartbeat(heartbeat); 
}

void loop() {
  Blinker.run();
}